#ifndef __OMNI_H__
#define __OMNI_H__

#ifdef MECANUM_H_GLOBAL
    #define MECANUM_H_EXTERN
#else
    #define MECANUM_H_EXTERN extern
#endif
		
		
/* the radius of wheel(mm) */
#define RADIUS 76
/* the perimeter of wheel(mm) */
#define PERIMETER 477.52f
/* wheel track distance(mm) */
#define WHEELTRACK 202.5 //
/* wheelbase distance(mm) */
#define WHEELBASE 202.5
		
#define LENGTH_A 202.5f //底盘长
#define LENGTH_B 202.5f //地盘宽
#define LENGTH_xiebian 286.0f //地盘宽		
		
/* gimbal is relative to chassis center x axis offset(mm) */
#define ROTATE_X_OFFSET 0
/* gimbal is relative to chassis center y axis offset(mm) */
#define ROTATE_Y_OFFSET 0

/* chassis motor use 3508 */
/* the deceleration ratio of chassis motor */
#define MOTOR_DECELE_RATIO (1.0f / 19.0f)
/* single 3508 motor maximum speed, unit is rpm */
#define MAX_WHEEL_RPM 8500 //8347rpm = 3500mm/s
/* chassis maximum translation speed, unit is mm/s */
//#define MAX_CHASSIS_VX_SPEED 3300 //8000rpm
//#define MAX_CHASSIS_VY_SPEED 3300
#define MAX_CHASSIS_VX_SPEED 1500 //底盘最大值
#define MAX_CHASSIS_VY_SPEED 1500

//#define SET_CHASSIS_VX_SPEED 1000 //8000rpm
//#define SET_CHASSIS_VY_SPEED 1000
#define SET_CHASSIS_VXY_SPEED 1300 //正常速度 
//#define SET_CHASSIS_VXY_SPEED 2600 //正常速度 
#define SET_CHASSIS_VXY_SPEED_UP 2600 //加速速度
#define SET_CHASSIS_WZ_SPEED -300 //正常速度
//#define SET_CHASSIS_WZ_SPEED -70 //加速
#define SET_CHASSIS_WZ_SPEED_UP -400 //加速
#define CHASSIS_SPEED_ACC 50
/* chassis maximum rotation speed, unit is degree/s */
//#define MAX_CHASSIS_VW_SPEED 300 //5000rpm
#define MAX_CHASSIS_VW_SPEED 500
//#define MAX_CHASSIS_VW_SPEED 500

#define MOTOR_ENCODER_ACCURACY 8192.0f

struct omni_structure
{
    float wheel_perimeter; /* the perimeter(mm) of wheel */
    float wheeltrack;      /* wheel track distance(mm) */
    float wheelbase;       /* wheelbase distance(mm) */
    float rotate_x_offset; /* rotate offset(mm) relative to the x-axis of the chassis center */
    float rotate_y_offset; /* rotate offset(mm) relative to the y-axis of the chassis center */
};

struct omni_position
{
    float v_x_mm;
    float v_y_mm;
    float rate_deg;
    float position_x_mm;
    float position_y_mm;
    float angle_deg;
};

struct omni_speed
{
    float vx; // forward/back
    float vy; // left/right
    float vw; // anticlockwise/clockwise
};

struct omni_gyro
{
    float yaw_gyro_angle;
    float yaw_gyro_rate;
};

struct omni
{
    struct omni_structure param;
    struct omni_speed speed;
    struct omni_position position;
    struct omni_gyro gyro;
    float  wheel_rpm[4];
    float  wheel_ops[4];
};

struct omni_motor_fdb
{
    float total_ecd;
    float speed_rpm;
};

void omni_position_measure(struct omni *omni, struct omni_motor_fdb wheel_fdb[]);
void omni_calculate(struct omni *omn);
//void moto_data_send(struct omni_motor_fdb wheel_fdb);

#endif 